// --------------------------------------------------------------------------------------------------------------------
// <copyright file="CubicSplineTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Test.Robotics.Manipulation
{
    using System;
    using Microsoft.Robotics.Manipulation;
    using Microsoft.Robotics.Manipulation.Runtime;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    /// <summary>
    /// Test <see cref="CubicSpline"/> class
    /// </summary>
    [TestClass]
    public class CubicSplineTest
    {
        /// <summary>
        /// Test to create a CubicSpline
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void CubicSplineConstructorTest()
        {
            CubicSpline cs = new CubicSpline(1, 2);
        }

        /// <summary>
        /// Test to create a CubicSpline
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void CubicSplineConstructorTest2()
        {
            CubicSpline cs = new CubicSpline(
                1,
                2,
                new double[2],
                new double[1,2],
                new double[1,2]);
        }

        /// <summary>
        /// Test to create a CubicSpline with invalid DOF
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [ExpectedException(typeof(ArgumentException))]
        public void CubicSplineConstructorInvalidDOFTest()
        {
            CubicSpline cs = new CubicSpline(0, 2);
        }

        /// <summary>
        /// Test to create a CubicSpline with invalid # of points
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [ExpectedException(typeof(ArgumentException))]
        public void CubicSplineConstructorInvalidPointsTest()
        {
            CubicSpline cs = new CubicSpline(1, 1);
        }

        /// <summary>
        /// Test to create a CubicSpline with an invalid first time
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [ExpectedException(typeof(ApplicationException))]
        public void CubicSplineConstructorInvalidFirstTimeTest()
        {
            CubicSpline cs = new CubicSpline(1, 2);
            cs.Times[0] = 1;
            cs.GetPositions(0.5);
        }

        /// <summary>
        /// Test to create a CubicSpline with invalid DOF
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [ExpectedException(typeof(ArgumentException))]
        public void CubicSplineConstructorInvalidDOFTest2()
        {
            CubicSpline cs = new CubicSpline(
                0,
                2,
                new double[1],
                new double[1,1],
                new double[1,1]);
        }

        /// <summary>
        /// Test to create a CubicSpline with invalid # of points
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [ExpectedException(typeof(ArgumentException))]
        public void CubicSplineConstructorInvalidPointsTest2()
        {
            CubicSpline cs = new CubicSpline(
                1,
                1,
                new double[1],
                new double[1,1],
                new double[1,1]);
        }

        /// <summary>
        /// Test to create a CubicSpline with incorrect DOF for target position
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [ExpectedException(typeof(ArgumentException))]
        public void CubicSplineConstructorInvalidTargetPosDOF()
        {
            CubicSpline cs = new CubicSpline(
                1,
                2,
                new double[2],
                new double[2,2],
                new double[1,2]);
        }

        /// <summary>
        /// Test to create a CubicSpline with incorrect DOF for target velocity
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [ExpectedException(typeof(ArgumentException))]
        public void CubicSplineConstructorInvalidTargetVelDOF()
        {
            CubicSpline cs = new CubicSpline(
                1,
                2,
                new double[2],
                new double[1,2],
                new double[2,2]);
        }

        /// <summary>
        /// Test to create a CubicSpline with incorrect # of points for target position
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [ExpectedException(typeof(ArgumentException))]
        public void CubicSplineConstructorInvalidTargetPosPoints()
        {
            CubicSpline cs = new CubicSpline(
                1,
                2,
                new double[2],
                new double[1,1],
                new double[1,2]);
        }

        /// <summary>
        /// Test to create a CubicSpline with incorrect # of points for target velocity
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [ExpectedException(typeof(ArgumentException))]
        public void CubicSplineConstructorInvalidTargetVelPoints()
        {
            CubicSpline cs = new CubicSpline(
                1,
                2,
                new double[2],
                new double[1,2],
                new double[1,1]);
        }

        /// <summary>
        /// Test to create a CubicSpline with incorrect # of points in times array
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [ExpectedException(typeof(ArgumentException))]
        public void CubicSplineConstructorInvalidTimePoints()
        {
            CubicSpline cs = new CubicSpline(
                1,
                2,
                new double[3],
                new double[1, 2],
                new double[1, 2]);
        }

        /// <summary>
        /// Test to create a CubicSpline with a non-zero value for first time (invalid)
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [ExpectedException(typeof(ArgumentException))]
        public void CubicSplineConstructorNonZeroFirstTimeValue()
        {
            CubicSpline cs = new CubicSpline(
                1,
                2,
                new double[2] { 1, 2 },
                new double[1, 2],
                new double[1, 2]);
        } 

        /// <summary>
        /// Test basic cubic spline functionality
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void CubicBasicFunctionTest()
        {
            CubicSpline cs = new CubicSpline(2, 3);
            cs.Times[0] = 0;
            cs.Times[1] = 1;
            cs.Times[2] = 2;

            cs.TargetPositions[0, 0] = 0;
            cs.TargetPositions[0, 1] = 10;
            cs.TargetPositions[0, 2] = 0;

            cs.TargetPositions[1, 0] = 30;
            cs.TargetPositions[1, 1] = 20;
            cs.TargetPositions[1, 2] = -20;

            cs.TargetVelocities[1, 1] = -25;

            double[] positions0000ms = cs.GetPositions(0);
            double[] positions0500ms = cs.GetPositions(0.5);
            double[] positions1000ms = cs.GetPositions(1.0);
            double[] positions1500ms = cs.GetPositions(1.5);
            double[] positions2000ms = cs.GetPositions(2.0);
            double[] positions2100ms = cs.GetPositions(2.1);

            // Check that various points returned by the spline are as expected
            Assert.AreEqual(positions0000ms[0], cs.TargetPositions[0, 0]);
            Assert.AreEqual(positions0000ms[1], cs.TargetPositions[1, 0]);

            Assert.AreEqual(positions1000ms[0], cs.TargetPositions[0, 1]);
            Assert.AreEqual(positions1000ms[1], cs.TargetPositions[1, 1]);

            Assert.AreEqual(positions2000ms[0], cs.TargetPositions[0, 2]);
            Assert.AreEqual(positions2000ms[1], cs.TargetPositions[1, 2]);

            // DOF '0' should be symmetric
            Assert.AreEqual(positions0500ms[0], positions1500ms[0], MathConstants.ErrorEpsilon);
            Assert.AreEqual(positions0000ms[0], positions2000ms[0], MathConstants.ErrorEpsilon);

            Assert.IsTrue(positions0000ms[1] > positions0500ms[1]);
            Assert.IsTrue(positions0500ms[1] > positions1500ms[1]);
            Assert.IsTrue(positions1500ms[1] > positions2000ms[1]);

            Assert.IsNull(positions2100ms);
        }

        /// <summary>
        /// Test for GetSplineIndex()
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void CubicSplineGetSplineIndexTest()
        {
            CubicSpline cs = new CubicSpline(
                1,
                3,
                new double[3] { 0.0, 1.0, 2.0 },
                new double[1, 3],
                new double[1, 3]);

            Assert.AreEqual(-1.0, cs.GetSplineIndex(-1.0));
            Assert.AreEqual(-1.0, cs.GetSplineIndex(2.1));
            Assert.AreEqual(1, cs.GetSplineIndex(1.5)); 
        }

        /// <summary>
        /// Test for FixIntermiatePointVelocities() when given positions and
        /// velocities of zero only
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void FixIntermediatePointVelocitiesZerosTest()
        {
            CubicSpline cs = new CubicSpline(
                1,
                2,
                new double[2] { 0.0, 1.0 },
                new double[1, 2],
                new double[1, 2]);

            cs.FixIntermediatePointVelocities(false);

            Assert.AreEqual(0, cs.TargetVelocities[0, 0]);
            Assert.AreEqual(0, cs.TargetVelocities[0, 1]);

            cs = new CubicSpline(
                1,
                3,
                new double[3] { 0.0, 1.0, 2.0 },
                new double[1, 3],
                new double[1, 3]);

            cs.FixIntermediatePointVelocities(false);

            Assert.AreEqual(0, cs.TargetVelocities[0, 0]);
            Assert.AreEqual(0, cs.TargetVelocities[0, 1]);
            Assert.AreEqual(0, cs.TargetVelocities[0, 2]);

            cs = new CubicSpline(
                2,
                3,
                new double[3] { 0.0, 1.0, 2.0 },
                new double[2, 3],
                new double[2, 3]);

            cs.FixIntermediatePointVelocities(false);

            for (int dof = 0; dof < 2; ++dof)
            {
                Assert.AreEqual(0, cs.TargetVelocities[dof, 0]);
                Assert.AreEqual(0, cs.TargetVelocities[dof, 1]);
                Assert.AreEqual(0, cs.TargetVelocities[dof, 2]);
            }
        }

        /// <summary>
        /// Test for FixIntermiatePointVelocities() at inflection points
        /// (i.e a point where the direction of motion changes)
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void FixIntermediatePointVelocitiesInflectionPointTest()
        {
            CubicSpline cs = new CubicSpline(
                1,
                3,
                new double[3] { 0.0, 1.0, 2.0 },
                new double[1, 3],
                new double[1, 3]);

            cs.TargetPositions[0, 1] = 1.0;

            cs.FixIntermediatePointVelocities(false);

            // we expect velocities to be zero at inflection points
            Assert.AreEqual(0, cs.TargetVelocities[0, 0]);
            Assert.AreEqual(0, cs.TargetVelocities[0, 1]);
            Assert.AreEqual(0, cs.TargetVelocities[0, 2]);

            cs.FixIntermediatePointVelocities(true);

            // we expect velocities to be zero at inflection points
            Assert.AreEqual(0, cs.TargetVelocities[0, 0]);
            Assert.AreEqual(0, cs.TargetVelocities[0, 1]);
            Assert.AreEqual(0, cs.TargetVelocities[0, 2]);

            cs = new CubicSpline(
                1,
                3,
                new double[3] { 0.0, 1.0, 2.0 },
                new double[1, 3],
                new double[1, 3]);

            cs.TargetPositions[0, 1] = 1.0;
            cs.TargetPositions[0, 2] = -1.0;

            cs.FixIntermediatePointVelocities(false);

            // we expect velocities to be zero at inflection points
            Assert.AreEqual(0, cs.TargetVelocities[0, 0]);
            Assert.AreEqual(0, cs.TargetVelocities[0, 1]);
            Assert.AreEqual(0, cs.TargetVelocities[0, 2]);

            cs.FixIntermediatePointVelocities(true);

            // we expect velocities to be zero at inflection points
            Assert.AreEqual(0, cs.TargetVelocities[0, 0]);
            Assert.AreEqual(0, cs.TargetVelocities[0, 1]);
            Assert.AreEqual(0, cs.TargetVelocities[0, 2]);


            cs = new CubicSpline(
                1,
                3,
                new double[3] { 0.0, 1.0, 2.0 },
                new double[1, 3],
                new double[1, 3]);

            cs.TargetPositions[0, 1] = 1.0;
            cs.TargetPositions[0, 2] = 0.5;

            cs.FixIntermediatePointVelocities(false);

            // we expect velocities to be zero at inflection points
            Assert.AreEqual(0, cs.TargetVelocities[0, 0]);
            Assert.AreEqual(0, cs.TargetVelocities[0, 1], MathConstants.ErrorEpsilon);
            Assert.AreEqual(0, cs.TargetVelocities[0, 2]);

            cs.FixIntermediatePointVelocities(true);

            // we expect velocities to be zero at inflection points
            Assert.AreEqual(0, cs.TargetVelocities[0, 0]);
            Assert.AreEqual(0, cs.TargetVelocities[0, 1], MathConstants.ErrorEpsilon);
            Assert.AreEqual(0, cs.TargetVelocities[0, 2]);
        }

        /// <summary>
        /// Test for FixIntermiatePointVelocities() using a set of values
        /// with a known answer verified in matlab (e.g. results are verified
        /// C2)
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void FixIntermediatePointVelocitiesKnownResultTest()
        {
            const double expectedPrecision = 0.00001;

            CubicSpline cs = new CubicSpline(
                1,
                4,
                new double[4] { 0.0, 20.0, 30.0, 40.0 },
                new double[1, 4] { { 0.0, 0.5331, 0.214, -0.4055 } },
                new double[1, 4] { { 0.0, 0.0, 0.0, 0.0 } });

            cs.FixIntermediatePointVelocities(false);

            Assert.AreEqual(cs.TargetVelocities[0, 0], 0.0, expectedPrecision);
            Assert.AreEqual(cs.TargetVelocities[0, 1], 0.0, expectedPrecision);
            Assert.AreEqual(cs.TargetVelocities[0, 2], -0.042122, expectedPrecision);
            Assert.AreEqual(cs.TargetVelocities[0, 3], 0.0, expectedPrecision);

            cs = new CubicSpline(
                1,
                10,
                new double[10] { 0, 6, 26, 28, 31, 32, 37, 40, 49, 50 },
                new double[1, 10] { { 0, 1, 3, 5, 4, 3, 4, 3, 2, 1 } },
                new double[1, 10] { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } });

            cs.FixIntermediatePointVelocities(false);

            Assert.AreEqual(cs.TargetVelocities[0, 0], 0.0, expectedPrecision);
            Assert.AreEqual(cs.TargetVelocities[0, 1], 0.125, expectedPrecision);
            Assert.AreEqual(cs.TargetVelocities[0, 2], 0.181818, expectedPrecision);
            Assert.AreEqual(cs.TargetVelocities[0, 3], 0.0, expectedPrecision);
            Assert.AreEqual(cs.TargetVelocities[0, 4], -0.50000, expectedPrecision);
            Assert.AreEqual(cs.TargetVelocities[0, 5], 0.0, expectedPrecision);
            Assert.AreEqual(cs.TargetVelocities[0, 6], 0.0, expectedPrecision);
            Assert.AreEqual(cs.TargetVelocities[0, 7], -0.16667, expectedPrecision);
            Assert.AreEqual(cs.TargetVelocities[0, 8], -0.20000, expectedPrecision);
            Assert.AreEqual(cs.TargetVelocities[0, 9], 0.0, expectedPrecision);
        } 

        /// <summary>
        /// Test for FixIntermiatePointVelocities() with <c>enforceZeroVelocityAtSecondPoint</c>
        /// set to true.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void FixIntermediatePointVelocitiesKnownResultEnforceZeroVelocityAtSecondPointTest()
        {
            CubicSpline cs = new CubicSpline(
                1,
                4,
                new double[4] { 0.0, 20.0, 30.0, 40.0 },
                new double[1, 4] { { 0.0, 0.5331, 0.214, -0.4055 } },
                new double[1, 4] { { 0.0, 0.0, 0.0, 0.0 } });

            cs.FixIntermediatePointVelocities(true);

            Assert.AreEqual(cs.TargetVelocities[0, 0], 0.0, 0.000001);
            Assert.AreEqual(cs.TargetVelocities[0, 1], 0.0, 0.000001);
            Assert.AreEqual(cs.TargetVelocities[0, 2], -0.042122, 0.000001);
            Assert.AreEqual(cs.TargetVelocities[0, 3], 0.0, 0.000001);
        }
    }
}
